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ABSTRACT 



In a tactical missile with mid-course guidance, an 
Inertial Navigation System (INS) is required. Strapdown INS 
using Steady-State Kalman Filters (SKF) as estimators has 
been suggested and this kind of INS is considered to be 
easier and cheaper to implement than the gimbaled INS. 

This thesis investigates one aspect of the INS problem: 
The sensitivity of the SKF to inaccuracies in the filter 
parameters such as the stability derivatives. The analysis 
has been performed by varying each of the flight parameters 
over a given range and noting the effect on the accuracy of 
the filter. The great advantage of the analysis in the 
sensitivity of rms estimate errors to inaccuracies in the 
stability derivatives is that it points out clearly which 
derivatives are worthy of detailed accurate determination and 
which are not. 
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I . INTRODUCTION 



In a typical tactical missile trajectory, standoff (or 
mid-course) guidance is required when the missile is launched 
at such long ranges from the target that either the missile 
seeker cannot detect the target or, if it can, the information 
is of such poor quality that is unusable. The mid-course 
guidance law usually consists of some pre-programed strategy 
such as to maintain the launch heading, constant altitude 
and speed. Mid-course guidance is primarily an inertial 
instrumentation problem. Typical sensors onboard missiles 
consist of three rate gyros (pitch, yaw and roll), accelero- 
meters, sometimes magnetic compass and some kind of altimeter. 
Additional state information can be provided by the seeker. 

A radar seeker could provide range and range rate. 

Here is considered the problem of using steady-state 
Kalman Filters as part of a navigation system, where the two 
estimators together (longitudinal and lateral estimators), 
constitute a strapdown system that does not use accelero- 
meters or gimbaled gyros. Strapdown Inertial Navigation 
Systems (INS) using Steady-State Kalman Filters (SKF) as 
estimators have been considered as cheaper and easier to 
implement than the gimbaled INS. The sensors used in this 
analysis are: pitch and roll rate gyros, altimeter and mag- 

netic compass. Since constant gain estimators have been 
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suggested as easier and cheaper to implement, the analysis is 
confined to the SKF. 

This work has been motivated by the idea given in Ref. 
M, where the author discusses in detail the configuration’ 
of such a navigation system as described before, but the 
problem is presented as a particular application in the INS 
of the DC-8 airplane. For convenience the model used here 
essentially is the same as Ref. [41 in lieu of a missile. 

This was done in order to eliminate classification problems. 

This thesis investigates only one aspect of the INS 
problem. That aspect is the sensitivity of the Kalman Filter 
to inaccuracies in the filter parameters or variation between 
the filter model and the plant model. These differences can 
be due to either inaccuracies as indicated or to a normal 
variation in the flight parameters due to different flight 
regimes. One result of this is the sensitivity of rms 
estimate errors to inaccuracies or differences in the sta- 
bility derivatives. 

The work was begun by reproducing the results given in 
Ref. [4] with the correct implementation of the dynamics in 
the filter parameters (i.e., Xu, Yv, etc.) over a given 
range and noting the effect on the accuracy of the filter. 
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II. STOCHASTIC MODELS AND ESTIMATION 



. A. KALMAN FILTER 

The Kalman Filter is widely documented and no attempt at 
a development of general theory has been made in this work. 

A brief description has been included in order to establish 
the particular formulation used. For a more complete de- 
velopment one is referred to Ref C1D. 

1 . Linear Dynamic System 

The formulation used employs state-space notation 
which offers the advantage of mathematical and notational 
convenience. Consider the linear time invariant system 
(system and measurement models) given by equation (1) , where 
x represents the states of the system; z is the 



measurement vector; F is the system matrix; r is the driving 
noise coefficient matrix; H describes the relationship between 
the states and the measurement; w and v are independent, zero- 
mean white gaussian noise process with covariance matrices Q 
and R respectively, that in mathematical terms are given by: 



x = Fx + Tw 



(1-a) 



z = Hx + v 



(1-b) 



E(w(t)w T (x) ) = Q(t) 6 (t-x) , E (w ( t ) ) = 0 
E(v(t)v T (x>) = R(t) 6 (t-x) , E(v(t) ) = 0 



(2-a) 



(2-b) 
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2 . Continuous Time Kalman Filter 



A continuous time Kalman Filter is described by 

A 

equation (3), where x is the state estimate; K is a matrix of 
constant filter gains. The implementation of the System 
Model and the Kalman Filter is shown in block diagram in 
Figure 1. 



x = Fx + K(z-Hx) (3) 



MATHEMATICAL_MODEL 

SYSTEM MEASUREMENT I 



KALMAN FILTER 




Figure 1 

System Model and Kalman Filter 
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The estimate error is defined by equation (4) as, 



~ A ~ 

x = x - x (4) 

and the differential equation for x is given by 
0 

x = (F-KH)x - rw + Kv (5) 

The differential equations for the state of a linear system 
driven by white noise can be expressed as 







r i n 




— 




m— — 


X 




F-KH ^ 0 




X 


+ 


Kv- Tw 


• 




0 ' F_ 




X 




Tw 



( 6 ) 



The covariance of the estimate-error, designated P, is given 
by equation (7) . It provides a statistical measure of the 
uncertainty in x. 



P = E[xx T 3 



(7) 



The diagonal elements of the covariance matrix are the mean 
square errors in the knowledge of the state variables. Also, 
the trace of P is the mean square length of the vector x. 

The off diagonal terms of P are indicators of cross-correlation 
between the elements of x. The covariance matrix P is obtained 
by solving the linear Lyapunov equation given by 
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( 8 ) 



P = (F-KH)P + P(F-KH) T + rQr T + krk t 
T he eigenvalues of the filter are the roots of equation (9) . 

[SI - F + KH| =0 (9) 

B. STATE AUGMENTATION AND SHAPING FILTERS 

When the system random disturbances are correlated in 
time, i.e., colored noise, it is necessary 'to use their 
power spectral density data in order to develop a mathematical 
model that produces an output which duplicates the noise 
characteristics [Ref. 23 . Correlated random noises are taken 
to be state variables of a fictitious linear time invariant 
system (generally called a shaping filter) which is itself 
excited by white gaussian noise. Such a model is given by 
equation (10), where the subscript f, denotes filter and n, 
is a nonwhite, i.e., time-correlated, gaussian noise. The 
filter output is used to drive the system as show in Fig. 2. 

O 

Xf = F^x^ + r f w (10-a) 

n = H^x^ (10-b) 

The dimension of the state vector (1) is increased by 
including the disturbances as well as a description of the 
system dynamics behavior in appropriate rows of an enlarged 
F matrix, this is called state vector augmentation. 
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I SHAPING FILTER SYSTEM 




J 



Figure 2 

Shaping filter generating driving noise. 
Conformation of the Augmented System. 



The augmented state equation is given by 




(ID 



The associated measurement equation given by equation (12) 



z 





+ v 



( 12 ) 



C. SENSITIVITY TO PARAMETER VARIATION 

Observing the structure of the Kalman Filter illustrated 
in Figure 1; the filter contains an exact model of the system 
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dynamics. The filter gain matrix is calculated using the 
parameters of the exact model of the dynamics. 

1 . Incorrect Implementation of Dynamics 

The analysis of how the error covariance behaves when 
the gain matrix is computed using wrong values of F matrix, 
i.e., varying parameters due to different flight conditions, 
is quite well explained in Reference HID. Figure 3 shows the 
block diagram for the system model and the Kalman Filter, 
where the quantities K* and F* represent the gain matrix and 
the system dynamics implemented in the filter. 



SYSTEM KALMAN FILTER 




Figure 3 

Block diagram of the System Model and Kalman 
Filter with incorrect implementation of dynamics. 
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The equation for the estimate is given by 



x = F*x + K*(z-Hx) (13) 



The error in the estimate given by (14) 



x - (F* - K*H)x + AFx - Tw + K*v 



(14) 



where 



AF 



A 

= F* - 



F 



The differential equations for the states of a linear system 
driven by white gaussian noise given previously by equation 
(6) becomes now 















— 


X 




F* - K*H AF 




X 


+ 


K*v - Tw 


o 

_x_ 




0 ! F _ 




X 




rw 



A 

Letting x' be the augmented state vector, x' = 
where the covariance matrix of x' is given by: 




E(x’x' T ) 




(15) 



(16) 



where 



P = E(x5c T ), V = E(xx T ), U £ E(xx T ) 

The quantity of interest is P, the covariance of x. 

The error sensitivity equations are given by 

P = (F* - K*H)P + P(F* - K*H) T + AFV 

+ v t af t + ror T + k*rk* t 



v = fv + v(f* - k*h) t + uaf t - rQr T 


(17-b) 


U = FU + UF T + TQr T 


(17-c) 



with initial conditions 

P(0) = -V(0) = U(0) = E (x(0)x(0) T ) 

when the actual system dynamics are faithfully reproduced in 
the filter, i.e., F = F*, AF = 0, the equation (17) reduces 
to the linear Lyapunov equation given by equation (8) . 

D. MODAL COORDINATES TRANSFORMATION 

The representation of the system given by equation (1) is 
not unique. Considering an alternative linear transformation 
of the states given in references C3] and C4-H , let x = T£, 
where £ represents the transformation of the states and T is 
the transformation matrix with the columns formed by the 
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eigenvectors of the system matrix F (for a complex eigenvalue, 
the first column is the real part and the second column is 
the imaginary part of the eigenvector) . The similarity 
transformation of equation (1) is given by 



c = Ac + Bw 


(18-a) 


z = CC + v 


(13-b) 



where 



A = T _1 FT, B = T~ 1 T , C = HT 

A case of particular interest (canonical form) occurs 
when the resulting A matrix is diagonal (the eigenvalues of 
the F matrix form the diagonal). The design of the estimator 
in modal coordinates is analogous to using transfer functions 
in partial fraction form. The canonical form is more inform- 
ative than the transfer frunction method, since observability 
and controllability can be obtained by inspection. 

E. SOLUTION OF THE KALMAN FILTER WITH A PRESCRIBED DEGREE 
OF STABILITY 

The Kalman Filter designed with constant gain (SKF) and 
used as an observer will diverge if there are undisturbed, 
neutrally stable (UNS) modes in the system model. In refer- 
ences C43 and C5U the authors presented the idea of 

\ 
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destabilization of the system model (1) , the amount of de- 
stabilization can be varied until the resulting suboptimal 
observer has a specified degree of stability. The method 
presented in Ref. C^H refers only to destabilizing the UNS 
modes in the system model, that method is called "modal 
destabilization" (MDS) . The gains of the filter are con- 
strained so that 

Re(Si) - -a, i = 1,2, n (19) 

where Re (Si) indicates "real part of (Si)," Si,....Sn are 
the eigenvalues of the filter, i.e., the roots of equation 
(9) and a is a specified positive number. 

The original system is destabilized in accordance with 
equation (20), where F' is the resultant destabilized matrix, 
E the destabilization matrix (diagonal) and T is the modal 
transformation matrix (eigenvector matrix). The matrix F r is 
used in order to calculate the modified gains of the filter 
(suboptimal gains) . 

F’ = F + TET" 1 (20) 

This method avoids the divergence of the steady-state 
Kalman Filter with a slight decrease in estimation accuracy. 
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III. DYNAMIC AND MEASUREMENT SYSTEM MODELS 



A. REFERENCE AXIS SYSTEM 

The Reference Axis System in a missile is centered on the 

c.g. and fixed on the body as follows: 

X axis, called the roll axis, forwards, along 
the axis of symmetry. 

Y axis, called the pitch axis, outwards and to 
the right if viewing the missile from behind, 

Z axis, called the yaw axis, downwards in the 
plane of symmetry to form a right-handed 
orthogonal system with the other two. 

The symbols from Appendix A define the forces and moments 
acting on the missile, the linear and angular velocities, and 
the moments of inertia; those quantities are shown in Figure 




Reference Axis System 
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B. MISSILE EQUATIONS OF MOTION 

The equations of motion used in the present study in order 
to represent the missile dynamics are quite well defined in 
Reference C7], A linear dynamical model of the missile based 
on the rigid body approximation is appropriate. 

1 . Longitudinal Motion 

The longitudinal motions of a missile are modeled by 
a fifth-order system given by equation (21) , where the state 
variables are u, velocity along the X axis; w, velocity along 
the Z axis; q, pitch rate, 0, pitch angle and h, altitude. 



“u~ 
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2 . Lateral Motion 

The lateral motions of a missile are modeled by a 
fifth-order system given by equation (22) , where the state 
variables defined are; 8 sideslip angle; r yaw rate; p roll 
rate; <j> roll angle; \p heading angle. 
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C. MODEL 

The aerodynamic data used in the present study is given 
in Appendix B [Ref 9], Essentially the models and noise 
dynamics are those of reference [4]. 

1 . Longitudinal Motion Estimation 

The main disturbance inputs are the two wind velo- 
cities u„ and w . Under the particular flight conditions, 

o o 

the turbulance represented by the fluctuating parts of Ug and 
Wg are color noise. They are modeled by first-order shaping 
filters with white gaussian noise inputs as given in equation 
(10) . The resultant linear model is given by equation (23) 
[Ref 8]. 



“g 
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_ y w. 



(23) 

The numerical data for the longitudinal dimensional 
derivatives was used in equation (21) , the resultant model is 
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given by equation (24) , that corresponds to the state vector 

augmentation given by equation (11) . Units are scaled with 

u, w, u and w in units of 10 ft/s, q in units of 0.01 rad/s. 
S & 
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0 in -units of 0.01 rad., and h in units of 100 ft. 

The measurement model given by equation (25) assumes 
a rate gyro in order to measure z^ and a barometric altimeter 
in order to measure z^. 
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2. Lateral Motion Estimation 



The main disturbance input is lateral wind Vg. The 
turbulence represented by the fluctuating part of Vg is color 
noise, it is modeled as a first-order shaping filter with 
white gaussian noise input as given in equation (10) . The 
resulting shaping filter is as given by equation (26) and 
taken from Reference C4G. 

6 = -0.8536 + 0.853y 

g g 

where (26) 

6 = v , 

g g/v 

The numerical data for the lateral dimensional derivatives 
was used in equation (22) to obtain the equation (27) , This 
corresponds to the state vector augmentation given by 
equation (11) . 
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The measurement model given by equation (28) , represents the 
situation where the measurement Zp is taking with a roll-rate 
gyro and the measurement using a magnetic compass. 
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IV. ANALYSIS 



A. THE SIMULATION 

A computer program was developed in order to solve the 
sensitivity equations given by equation (17) . Basically, the 
program was used to handle a set of 105 linear differential 
equations in the longitudinal case and 78 in the lateral. 

The principle program output is the time history of the P 
matrix and the square root of its diagonal elements (the rms 
estimate errors) , 

The Kalman Filter computer program was the primary tool 
used in computing the gains. The program was developed at 
the Massachusetts Institute of Technology CRef. 10D and 
adapted for use at the Naval Postgraduate School. A second 
computer program composed at Stanford University, OPTSYS 4 
Computer Program CRef. HD, which utilizes several options of 
the Kalman Filter, was also implemented in this analysis. In 
particular, the suboptimal filter using the modal destabili- 
zation (MBS) design option was required for the lateral case. 

B . THE RESULTS 

As the problem is described, the results are presented in 
two parts, the longitudinal and the lateral case. 

Initially both models given by equations (24) and (27) 
were modified in order to include another state in each of 
them, the perturbed position. The resultant observers were 
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tested, but in both cases they were unstable, 

1 , Longitudinal Motion Estimation Analysis 

A steady-state Kalman Filter was designed ERef, 4] 
and used as a constant gain state estimator (equation (3)), 

The first step in this analysis was to reproduce the 
results for the case of exact implementation of dynamics in 
the calculation of the Kalman gain matrix. That analysis was 
done using the M.I.T. Kalman Filter Computer Program [Ref, 10U. 
The results are shown below: 



Filter 


gain matrix K. 


0.059 


0.060 


0 . 264 


-0.161 


3.517 


0.040 


0.001 


-0.080 


0.013 


0.136 


-0.011 


0.035 


-1.288 


0.128 



Filter Eigenvalues 
-0.310 + j0.411 
-0.429 
-0.178 
-0.261 

-0.063 + jO .0743 

rms estimate errors 
u = 2.090 ft/s 
w = 5.102 ft/s 
q = 0.416 deg/s 



6 = 0.317 deg 
h = 8.245 ft 
ug = 4.776 ft/s- 
Wg = 5.701 ft/s 
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These results are essentially identical to those of Ref. [4]. 

The next objective was to consider the incorrect 
implementation of dynamics in the Kalman Filter, in order to 
study the sensitivity of rms estimate errors to inaccuracies 
in the stability derivatives. The numerical variation of the 
parameters were considered individually in order to establish 
the F* matrix to be used in equation (17) and also for finding 
the filter gain matrix K* to be implemented in the same 
equation. For each variation of the parameter under obser- 
vation it was necessary to run the M.I.T. Computer Program 
HRef. 103 in order to find the matrix of gains K* and then to 
run the sensitivity covariance program in order to study the 
propagation of the estimate errors. 

The results are shown in Tables 1-8 and Figures 5-40. 
In the Tables, the true value of each parameter is marked with 
an asterisk. The description of the results are: 

Xu . The dimensional variation of X force with forward 
speed (u) has a nominal value of -0.015. This quantity was 
changed in a range of +20%. The behavior of the rms estimate 
errors are presented in Table 1. From that it can be seen 
that any numerical variation of Xu derivative does not cause 
significant changes in the nominal values of the rms estimate 
errors of the states, w, q, 0, Ug and Wg. For the states 
u and h the results are shown in Figures 5-6 respectively, 
with some effect, but not significant for positive variation 
of Xu. 
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Xw. The dimensional variation of X force with down- 
ward speed (w) has a nominal value of 0.004. As before, the 
numerical variation was performed in a range of +20%, The 
behavior of the rms estimate errors were tabulated in Table 
2. Checking the results there is no variation in the nominal 
values of rms estimate errors of the states, w, q, 0, Ug and 
for any of the changes made in Xw derivative. The vari- 

o 

ations in the estimate errors for u and h are indicated in 
Figures 7 and 8 respectively. Increasing the value of Xw the 
u error decreases but h increases, while taking smaller values 
of the parameter, the effect is contrary. Those changes in 
the rms errors are considered not important. 

Zu. The dimensional variation of Z force caused by 
a change in forward speed (u) has a nominal value of -0.074. 
The effects of changing its design value was observed in a 
range of 20% and its result condensed in Table 3 as well as 
in Figures 9-14. All the rms errors show some degree of 
sensitivity except q. The most important changes occur in 
u, 0, and h errors and the large variation in u can begin to 
be important in terms of accuracy in radial position. 

Zw . The dimensional variation of Z force with down- 
ward speed (w) has a nominal value of -0.806. The results 
for this case are presented in Table 4 and Figures 15-20, 
Again, the variation in the value of Zw was made in a range 
of +20%. . All the rms estimate errors show a large degree of 
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sensitivity and it can be considered that any numerical vari- 
ation of Zw beyond + 2% is critical and unacceptable. 

Hu . The dimensional variation of M moment caused by 

« 

a change in forward speed (u) has a nominal value of -0.000786. 
The value of Hu was changed in a range of 20% and the results 
are given in Table 5 as well as Figures 21-26. Any variation 
in Hu quantity does not cause important changes in the value 
of the rms estimate errors of the states, q, Ug and Wg„ For 
For the states u, w, 0, and h, the variation in the errors 
can be considered with some significant effect for large 
deviations of Hu value, i.e., more than +10%. 

Hw . The dimensional variation of H moment with speed 
(w) has a nominal value of -0.0111. Its numerical variation 
was taken in a range of +107 o . From Table 6 and Figures 27-32, 
it can be seen that any alteration in the true value of Hw is 
reflected with a strong effect on all the rms estimate errors. 
This derivative can be considered the most critical in the 
longitudinal motion estimation case. 

Hq . The dimensional variation of pitching moment with 

i 

pitch rate (q) has a nominal value of -0.924. The results 
are given in Table 7 and Figures 33 and 34 for a range of 
+20% in the numerical variation of Hq. The sensitivity of the 
rms estimate errors due to changes in this parameter is min- 
imum for all the states. 
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Mw . The dimensional variation of pitching moment 
with rate of change of downward speed (w) has a nominal value 
of -0.00051. Table 8 and Figures 35-40 presents the infor- 
mation about the changes of the rms estimate errors in a 
range of +20% in the variation of Mw derivative. All the 
errors show some degree of sensitivity and with more than +2% 
in the alteration of the Mw value, the variation in the errors 
begins to be significant. 

2 . Lateral Motion Estimation Analysis 

The dynamic system (equation (27)) and measurement 
models (equation (28)) were transformed into modal coordin- 
ates in accordance with equation (18) . This was done using 
the OPTSYS 4 Computer Program [Ref. 113; the results are 
given below: 



£di 




-0.119 


1.490 


• 

£D2 




-1.490 


-0.119 


CR 




0 


0 


is 




0 


0 






0 


0 


• 

£vg 




0 


0 



0 0 0 0 

0 0 0 0 

-1.254 000 

0 -0.0041 0 0 

0 0 0 0 

0 00 -0.853 



?D1 




-1.702 


£D2 




-0.610 


?R 


+ 


-5,106 


£s 




-0.004 






0 


£vg 




3.788 



( 29 ) 



34 



5D1 
CD 2 



~z 

p 




1 0 -0.782 0 1 -0.605 




CR 


+ 


1 0 V? 


Z 1 




0.276 0.295 -0.008 0.995 1 0.162 




CS 




_° 1 v <|> 



CH 



( 30 ) 




These results are identical to those given in Ref. 

C4H. In the same reference the author gives a complete 
analysis of controllability and observability using the re- 
sults given in equations (29) and (30) . From these can be 
seen that the spiral mode CS has its eigenvalue close to the 
origin and it is almost undisturbed by the process noise; also 
it is unobservable with the measurement Zp. Using a steady- 
state Kalman Filter as an observer fail to estimate the 
heading mode C^ and its estimation of the spiral mode CS is 
too slow. The next objective was to take the SKF and to con- 
sider the situation of parameter variations in the dynamic 
implemented in the filter. With the true values of the F 
matrix implemented in the filter the results are shown below: 



Filter Gain Matrix K 


0.051 


-0.967 


-1.536 


0.411 


2.695 


-0.004 


0.386 


-0.789 


-0.005 


0.906 


-1.713 


0.655 
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Filter Eigenvalues 



-2.350 + j 2 . 594 
-0.624 + jO .492 
-0.00125 
0.0 



rms Estimate Errors 

V8 = 3.329 ft/s 
r = 0.244 deg/s 
p = 0.377 deg/s 
$ = 0.222 deg 
ip = 0.214 deg 
VBg = 5.506 ft/s 

These results are essentially identical to those 
from Ref. C43. The filter was found to be stable but with a 
narrow margin of stability. Any attempt to change parameters 
resulted in a failure of the estimator due to divergence with 
the most minimum variation (i.e., +17,) of any of the para- 
meters. This result complements the analysis given previously 
[Ref. 4U and it shows definitely that it is impossible to use 
the SKF as an estimator for the lateral motion with the actual 
configuration of the model given by equations (27) and (28) . 

The alternative solution was to design a suboptimal 
observer (MDS) with the constrain (19) as given in Ref. C4|], 
a = 0.029 and using equation (20) in the MDS option the 
computer program OPTSYS 4 [Ref, 11U. The results are: 
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Filter Gain Matrix K 



-0.073 


-0.871 


-1.560 


0.430 


2.731 


-0.033 


0.339 


-0.753 


-0.033 


0.928 


-1.622 


0.585 


Filter 


Eigenvalues 



-2.350 + j 2 . 589 
-0.624 + j0.492 
-0.0291 + j 0 . 005 



rms Estimate Errors 



V6 = 3.799 


ft/ s 


r = 0.245 


deg/s 


p = 0.379 


deg/s 


<f> = 0.226 


deg 


\p = 0.216 


deg 


Vgg = 5.712 


ft/s 



These results are essentially identical to those 
from Ref. C4H. 

The sensitivity of the lateral rms estimate errors 
due to inaccuracies in the stability derivatives was studied 
using the suboptimal filter (MDS) design and following the 
same procedure explained before for the longitudinal case. 
The results are shown in Tables 9-15 and Figures 41-76. The 
results are described in detail as follows: 



37 



Yv . The dimensional variation of Y force with side 
velocity (v) has a nominal value of -.0868. This value was 
changed in a range of +207, and the results appear in Table 9 
and Figures 41-44. r, p, ip, and V8g show almost no sensi- 
tivity to the variation in Yv parameter, V8 changes but it 
can be considered of secondary importance. The most sensi- 
tivity change occurs in <j> and it begins to be important for a 
variation of +57, in Yv value. 

N'. The dimensional variation of yawing moment about 

P 

Z axis with sideslip angle (6) has a nominal value of 2.14. 

Table 10 and Figures 45-50 give the results for a variation 

of N' derivative in a range of +107o. All the rms estimate 
p 

errors except \p present strong sensitivity, especially VS 
and b* With a variation of more than +17, the estimation of 
these errors begin to show large values and the filter takes 
a long time to reach the steady state value (more than 150 s) . 
For that reason, any variation in the true value of with 
the purpose of implementation in a Kalman Filter as an esti- 
mator must be considered unacceptable, 

N£. The dimensional variation of yawing moment about 
Z axis with yaw rate (r) has a nominal value of -0.228. In 
a range of +20% the variation of value was performed and 
the results tabulated in Table 11 as well as the important 
variations registered in Figures 51-54. The effects on 
sensitivity of all the rms estimate errors are secondary and 
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there is no important alteration in their values due to large 
variations in the N^. parameter. 

N]J. The dimensional variation of yawing moment about 
Z axis with roll rate (p) has a nominal value of -0.0204. 

This number was changed in a range of +20% and the results 
observed are given in Table 12 and Figures 55-60. r, p, 4 > , 
and V6g rms errors do not present any important variation. 

The sensitivity in rms estimate error is observed in Vg and 
b, in particular. For the last one, the change is too large 
for a variation of 5% in the value of the parameter. 

L& . The dimensional variation of rolling moment 
about X axis with sideslip angle (6) has a nominal value of 
-4.41. Its variation was given just in a margin of +10%, be- 
cause with larger changes the estimator starts to diverge. 

The results obtained are given in Table 13 and Figures 60-66. 
All the rms estimate errors show some degree of sensitivity 
due to the variation in Lg parameter and with 1%, of change 
some of the errors like Vg" and b present very large vari- 
ations. For that reason this derivative must be considered 
critical in the implementation of the Kalman Filter as an 
estimator . 

Lr. The dimensional variation of rolling moment 
about X axis with yaw rate (r) has a nominal value of -0.334. 
The results presented in Table 14 and Figures 67-72 are the 
product of the observation for the variation of the parameter 
in a range of +10% and it can be considered as the most 
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important stability derivative for the lateral model, because 
the dynamics implemented in the filter does not allow any 
variation of its true value, otherwise, the estimation of the 
errors becomes to be too large and probably the filter can 
diverge. In other words, the sensitivity of the estimator 
to any variation in L^. value is extremely critical. 

L£. The dimensional variation of rolling moment 
about X axis with roll rate has a nominal value of -1.204. 

The behavior of the rms estimate errors were observed taking 
a variation of +107> of the L^> parameter and they are given 
in Table 15 and Figures 73-76. VS and ? are the variables 
that present some degree of sensitivity in the increment of 
their rms estimate errors, respectively, and they begin to be 
important with a variation of more than +5% of the L]i> true 
value . 
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U RMS ESTIMATE ERROR VS XU 




Figures 5 & 6 . Sensitivity of u and h rms estimate errors vs 
variation in Xu stability derivative 



H RMS ESTIMATE ERROR VS XU 
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CM 



U RMS ESTIMATE ERROR VS XN 




Figures 7 & 8 . Sinsitivity of u and h rms estimate errors vs 
variation in Xw stability derivative. 

§ H RMS ESTIMATE ERROR VS XN 
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U RMS EST1MRTE ERROR VS ZU 




Figures 9 & 10. Sensitivity of u and w rms estimate errors vs 
variation in Zu stability derivative. 



W RMS ESTIMRTE ERROR VS ZU 
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H RMS ESTIMATE ERROR VS ZU 




Figures 11 & 12 ♦ Sensitivity of h and 9 rms estimate errors vs 

variation in Zu stability derivative. 



THETA RMS ESTIMATE ERROR VS ZU 




44 



WG RMS ESTIMATE ERROR VS ZU 




Figures 13 & 14 , Sensitivity of Wg and rms estimate errors vs 

variation of Zu stability derivative, 

uo 



UG RMS ESTIMATE ERROR VS ZU 
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N (FT/S) h u (FT/S) 

.00 5.50 6.00 6.50 fc' ,0.00 10.00 20.00 30.00 



U RMS ESTIMATE ERROR VS ZN 




ZN 

15 & 16 , Sensitivity of u and w rms estimate errors vs 
variation in Zw stability derivative. 



N RMS ESTIMATE 

Figure 16 



ERROR VS ZN 




ZN 
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THETfl (DEG) Q (DEG/S) 

0.25 0.35 0.45 b ,0.40 0.43 0.46 0.49 



Q RMS ESTIMRTE ERROR VS ZW 




ZN 

17 & 18 . Sensitivity of q and q nns estimate errors vs 



variation in Zw stability derivative. 

THETR RMS ESTIMRTE ERROR VS ZW 




ZW 
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UG (FT/S) • I* H (FT) 

.76 4.77 4.78 4.79 In' ,0.00 12.00 24.00 36.00 




1.00 -0.93 -0.86 -0/79 -0.72 -0.65 

ZN 

res 19 & 20 . Sensitivity of h and rms estimate errors vs 
variation in Zw stability derivative. 



UG RMS ESTIMATE ERROR VS ZN 
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U RMS ESTIMATE ERROR VS MU 
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ures 21 & 22 . Sensitivity of u and w rms estimate errors vs 
variation in Mu stability derivate. 



N RMS ESTIMATE ERROR VS MU 

Figure 22 
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H (FT) M THETA (DEG) 
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THETfl RMS ESTIMATE ERROR VS MU 

Figure 23 
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;ures 23 & 24 . 



Sensitivity of q and h rms estimate errors vs 
variation in Mu stability derivative. 



H RMS ESTIMATE ERROR VS MU 
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NG RMS ESTIMATE ERROR VS MU 

Figure 25 




1.00 - 0.92 - 0.84 - 0.76 - 0.68 - 0.60 

mu *icr 3 



;ures 25 & 26 , 



Sensitivity of Wg and Ug rms estimate errors vs 
variation in Mu stability derivative* 



UG RMS ESTIMATE ERROR VS MU 

Figure 26 
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1 .20 
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ures 27 & 28 . 



Sensitivity of u and w rms estimate errors vs 
variation in Mw stability derivative. 



N RMS ESTIMATE ERROR VS MN 
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Q RMS ESTIMATE 

Figure 29 



ERROR VS MN 
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;ures 29 & 30 . 



Sensitivity of q and 0 ms estimate errors vs 
variation in Mw stability derivative. 



THETA RMS ESTIMATE ERROR VS MN 




MN *10' 2 
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H RMS ESTIMRTE ERROR VS MW 

Figure 31 
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Figures 31 & 32 . Sensitivity of h and w rms estimate errors vs 

variation in Mw stability derivative. 



WG RMS ESTIMRTE ERROR VS MW 

Figure 32 
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RMS ESTIMRTE ERROR VS MQ 

Figure 33 
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Sensitivity of u and h rms estimate errors vs 
variation in Mq stability derivative. 



H RMS ESTIMATE ERROR VS MQ 

Figure 34 
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U RMS ESTIMATE ERROR VS MWDOT 



Figure 35 
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ures 35 & 36 . 



Sensitivity of u and w rms estimate errors vs 
variation in stability derivatives. 



W RMS ESTIMATE ERROR VS MWD0T 



Figure 36 
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Q RMS ESTIMATE ERROR VS MWOOT 

Figure 37 
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Sensitivity of q and 0 rms estimate errors vs 
variation in Mw stability derivative. 
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H RMS ESTIMATE ERROR VS MWDOT 




Figures 39 & 40 . Sensitivity of h and w g rms estimate errors vs 

variation of Mw stability derivative. 



WG RMS ESTIMATE ERROR VS MWDOT 
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VB RMS ESTIMRTE ERROR VS TV 

Figure 41 
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Sensitivity of Vg and p rms estimate errors vs 
variation in Yv stability derivative. 



P RMS ESTIMATE ERROR VS TV 

Figure 42 
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PHI RMS ESTIMATE ERROR VS TV 

Figure 43 
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Sensitivity of 0 and Vgg rms estiamte errors vs 
variation in Yv stability derivative. 



VBG RMS ESTIMATE ERROR VS TV 

Figure 44 
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VB RMS ESTIMATE ERROR VS NB ’ 

Figure 45 




2.40 



Figures 45 & 46 . Sensitivity of Vg and r rms estimate errors vs 

variation in Ng stability derivative. 



R RMS ESTIMATE ERROR VS NB’ 
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P RMS ESTIMATE ERROR VS NB 1 
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47 & 48 . Sensitivity of p and 0 rms estimate errors vs 
variation of stability derivative. 

PHI RMS ESTIMATE ERROR VS NB’ 

Figure 48 
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Figure 49 
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Sensitivity of and rms estimate errors vs 

variation in N* stability derivative. 
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Sensitivity of Vg and r rms estimate errors vs 
variation in stability derivative. 
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res 53 & 54 . Sensitivity of 0 and Vg rms estimate errors vs 
variation in stability derivative. 
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Figure 55 
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Sensitivity of Vg and r ms estimate errors vs 

variation in N f stability derivative. 
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Figure 56 
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Figure 57 
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Sensitivity of p and 9 rms estimate errors vs 

variation of N' stability derivative. 

P 



PHI RMS ESTIMATE ERROR VS NP 1 

Figure 58 
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Figure 59 
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Sensitivity of \p and Vg rms estimate errors vs 
variation in stability derivative. 
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Figures 61 & 62 . Sensitivity of Vg and r rms estimate errors vs 

variation stability derivative. 



co 

o 



R RMS ESTIMATE ERROR VS LB’ 

Figure 62 



cn 



=r 

OJ 

o 




CCO 



- 5.00 




- 4.80 - 4.60 - 4.40 - 4.20 

LB ' 



- 4.00 



69 



P RMS ESTIMRTE ERROR VS LB' 




F ig _ u res 63 & 64 . Sensitivity of p and 0 rms estimate errors vs 

variation in stability derivative. 
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Figure 65 




5. 00 - 4.80 - 4.60 - 4.40 - 4.20 ^ 4.00 

LB ’ 

^■ s 65 & 66. Sensitivity of $ and Vg g rms estimate errors vs 
.variation in stability derivative. 

VBG RMS ESTIMATE ERROR VS LB 1 

Figure 66 




-4.00 



LB 1 



71 



o 



VB RMS ESTIMATE ERROR VS LR 1 




Figures 67 & 68 . Sensitivity of Vg and r rms estimate errors vs 

variation of stability derivative. 
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69_&_70. Sensitivity of p and 0 rms estimate errors vs 
variation of L/ stability derivative. 
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Figure 71 
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Sensitivity of \p and Vg rms estimate errors vs 
variation of 1/ stabiltxy derivative. 
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Sensitivity of Vg and r rms estimate errors vs 
variation in L/ stability derivative. 
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Figure 75 
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Sensitivity of Q and V$ rms estimate errors vs 
variation in stability derivative. 
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V. CONCLUSIONS AND RECOMMENDATIONS 



A. CONCLUSIONS 

Using steady-state Kalman Filters as observers in an 
inertial navigation system, the sensitivity of rms estimate 
errors to inaccuracies in the stability derivatives was 
studied. The conclusions reached at the end of this research 
are presented in two parts as they were analyzed. 

1 . Longitudinal Motion Estimator 

a. Numerical variations in the dimensional deriv- 
atives; Xu, Xw and Mq do not show any important effect in the 
rms estimate errors. The value of these parameters as imple- 
mented in the filter can have large variations with respect 
to their nominal values (perhaps a tolerance of +20%) . 

b. Changes in the stability derivatives; Zu, Mu 
and Mw are reflected with variation in almost all the rms 
estimate errors. In particular, large variations in u rms 
estimate error can begin to be important in terms of accuracy 
in the radial position. These parameters can be implemented 
in the dynamics of the filter with some degree of tolerance 
with respect to their true value (for the case under analysis 
no more than +5%) . 

c. The most important effects are found with the 
variations of the stability derivatives; Zw and Mw. Zw gives 
the strongest alterations in all the rms estimate errors and 
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small changes (+2% the tolerance in the case under study) in 
this parameter are unacceptable in the filter. For Mw the 
situation is quite similar and the value of this derivative 
is very important. In general, Zw and Mw must be faithfully 
reproduced in the filter. 

2 . Lateral Motion Estimator 

It is not possible to use the SKF as an observer 
under the particular configuration of the models given for 
lateral motion. The filter is stable but with a narrow margin 
of stability. The most minimum numerical variation (i.e., 

+1%) in the value of any stability derivative results in a 
failure of the filter due to divergence. This result com- 
plements the discussion given in Ref. C4-U. Using a sub- 
optimal filter (with modified gains) , the effect of the 
numerical variation in each of the stability derivatives 
are: 

a. Numerical variations in the dimensional deriva- 
tives; Yv, N^, and Lp do not cause important effects in any of 
the rms estimate errors. The reproduction of these para- 
meters in the filter are not required to be too accurate 
(considering in this case a tolerance of +10%) . 

b. Np is a parameter that only accepts small vari- 
ations in its value (in the problem discussed, 2% of toler- 
ance) because it has strong effects in the estimation of <j> 
rms estimate error. 
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c. The stability derivative Ng is one of the most 
important in terms of faithful reproduction in the dynamics 
of the filter. In general, it can be said that any numerical 
variation in its true value is not allowed. 

d. Finally, Lg and present the most dramatic 
situation, because a minimum change in its value is reflected 
with very large changes in the estimation of rms errors and 

it can result in divergence of the Kalman Filter. The lateral 
motion estimator design requires that these stability deriva- 
tives must be reproduced in the filter with the maximum of 
accuracy, otherwise, the result can be failure of the filter. 

Of the two estimators used in the implementation of 
the INS, the lateral filter is more sensitive to variation in 
its parameter value. It should be remembered that the con- 
clusions reached in this analysis apply specifically to the 
model being analyzed. Other models involving different 
missiles may indicate different sensitivities. 

Nevertheless, it is felt that the conclusions presented 
in this thesis are of general validity. 

B. SUGGESTIONS FOR FURTHER ANALYSIS 

The most obvious extension of the work would be to test 
the estimators using the real data for a missile. 

Further study could be to consider the same problem de- 
signing the estimators with a model that includes the addition 
of the estimation in position in order to analyze the problem 



79 



in terms of miss distance in radial position for a missile 
that at the end of the mid-course guidance phase must hit a 
"basket" (terminal guidance phase) , 
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VI . SUMMARY 



The great advantage of the analysis in the sensitivity 
of rms estimate errors to inaccuracies in the stability 
derivatives is that it points out clearly which derivatives 
are worthy of detailed accurate determination and which are 
not . 

The following table summarizes the level of importance of 
each of the stability derivatives in the implementation of 
the steady-state Kalman Filters to be used as estimators in 
an INS. 
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DERIVATIVE 



NI SI PI 



o 

w 

H 

2 

£ 

H 

Q 

P 

H 

M 

o 

p 

o 

P 



p 

o 



H 

GO 

W 



Xu 

Xw 

Zu 

Zw 

Mu 

Mw 

Mq 

Mw 



X 

X 



X 



X 



X 



X 



X 



X 



2 

o 

H S 

H o 

S M 
SH 



W CO 
E-t W 
< 

•J 



Yv 



N 6 

Nr 

N P 

H 

L r 

L’ 

P 



X 



X 



X 

X 

X 

X 



X 



NI = Not Important 

SI = Secondary (Relative) Importance 
PI = Primary Importance 
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APPENDIX A 
LIST OF SYMBOLS 



Regular Symbols 

A 

B 

C 

D 

E 

F 

f 

F' 

g 

H 

H 

h 

INS 

K 

L 

M 

MDS 

N 

n 

P 

P 

Q 

q 

R 

r 

S 

SKF 

T 



Definition 

Modal transformation of F matrix 
Modal transofrmation of F matrix 
Modal transformation of H matrix 
Dutch roll mode 
Destabilization matrix 
System dynamics matrix 
Subscript for filter 
Destabilized matrix 
Subscript for wind speed 
Measurement matrix 
Heading Mode 
Altitude 

Inertial Navigation System 

Kalman Filter gain matrix 

Rolling moment (about X axis) 

Pitching moment (about Y axis) 

Modal destabilization 

Yawing moment (about Z axis) 

Non-white gauss ian noise 

Covariance propagation of the estimate 
error matrix 

Perturbed roll rate 

Covariance matrix of w 

Perturbed pitch rate 

Covariance matrix of v 

Perturbed yaw rate 

Spiral mode 

Steady-state Kalman Filter 
Transformation matrix 
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Regular Symbols 
UNS 



Definition 



u 

v 

v 

w 

w 

X 

X 

A. 

X 

X 

Y 

z 

z 



Undisturbed neutrally stable 

Perturbed forward speed (along X axis) 

Forward velocity 

Perturbed side velocity 

Driving white gaussian noise 

Perturbed downward velocity 

Reference axis 

State vector of the system 

State estimate vector 

Estimate error vector 

Reference axis 

Measurement vector 

Reference axis 



Greek Symbols 

Q 

* 

0 

r 

a 

a 

S 



Definition 
Heading Angle 

Perturbed pitch attitude angle 
Perturbed bank (roll) angle 
Sideslip angle 
Driving noise matrix 
Eigenvalue constrain 
Standard deviation 
Transformed state vector 
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APPENDIX B 

AERODYNAMIC DATA AND PROBABILISTIC INFORMATION 

v = 820 ft/s 

1 . Longitudinal Model 

a. Dimensional Derivatives 



Xu 


=■ 


-0.015 


1/s 


Xw 


= 


0.004 


1/s 


Zu 




-0.074 


1/ s 


Zw 


= 


-0.0806 


1/s 


Mu 


= 


-0.0786 


1/s-f t 


Mw 


= 


-0.0111 


1/s-f t 


Mq 


= 


-0.924 


1/ s-rad 


Mw 


= 


-0.00051 


1/ft 



b. Disturbance Noise Standard Deviation 

cr u - a w = 1.15 1/s (10 ft/s) 2 

(7 ft/s rms gust with a 930-ft correlation 
distance) . 

c. Observation Noise Standard Deviation 
tq = 0.15 s (0.01 rad/s) 

cr h = 0.05 s (100 ft) 2 

2 . Lateral Models 

a. Dimensional Derivatives 

Y v = -0.0868 1/s 
Ng = 2.14 1/s 2 

N£ = -0.228 1/s 

Np = -0.0204 1/s 
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LJ = -4.41 1/s 2 

L£ = 0.034 1/s 

Lp = -1.181 1/s 

b. Disturbance Noise Standard Deviation 

0 = 1.63x10 ^ 1/s (7 ft/s rms gust with a 

930 ft correlation distance) 

c. Observation Noise Standard Deviation 
Op = 1.5x10 ^ s 

= 1.5xl0~ 5 1/s 
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COMPUTER OUTPUT 



Table 1. rms estimate errors for longitudinal motion estimator 
with variation in Xu derivative. 



Xu 


u 

ft/s 


w 

ft/s 


q 

deg/ s 


9 

deg 


h 

ft 


u 

§ 

ft/s 


ft/s 


-0.018 


2.096 


5.102 


0.416 


0.317 


8.248 


4.776 


5.701 


-0.0165 


2.094 


5.102 


0.416 


0.317 


8.246 


4.776 


5.701 


-0.01575 


2.091 


5.102 


0.416 


0.317 


8.240 


4.776 


5.701 


-0.015 * 


2.090 


5.102 


0.416 


0.317 


8.245 


4.776 


5.701 


-0.01425 


2.088 


5.103 


0.416 


0.317 


8.260 


4.775 


5.701 


-0.0135 


2.089 


5.103 


0.416 


0.317 


8.280 


4.775 


5.701 


-0.012 


2.092 


5.103 


0.416 


0.317 


8.340 


4.775 


5.701 



Table 2 . rms estimate errors for longitudinal motion estimator 
with variation in Xw derivative. 



Xw 


u 

ft/ s 


w 

ft/s 


q 

deg/s 


0 

deg 


h 

ft 


u g 
ft/ s 


w 

§ 

ft/s 


0.0048 


2.070 


5.102 


0.416 


0.317 


8.319 


4.776 


5.701 


0.0044 


2.080 


5.102 


0.416 


0.317 


8.282 


4.776 


5.701 


0.0042 


2.086 


5.102 


0.416 


0.317 


8.257 


4.776 


5.701 


0.004 * 


2.090 


5.102 


0.416 


0.317 


8.245 


4.776 


5.701 


0.0038 


2.100 


5.102 


0.416 


0.317 


8.223 


4.776 


5.701 


0.0036 


2.103 


5.102 


0.416 


0.316 


8.215 


4.776 


5.701 


0.0032 


2.110 


5.101 


0.416 


0.316 


8.170 


4.776 


5.701 
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Table 3 . rms estimate errors for longitudinal motion estimator 
with variation in Zu derivative. 



Zu 


u 

ft/s 


w 

ft/s 


q 

deg/s 


0 

deg 


h 

ft 


u 

ith 


ft 

ftfs 


-0.0888 


1.885 


5.106 


0.416 


0.322 


9.310 


4.776 


5.707 


-0.0814 


1.974 


5.104 


0.416 


0.319 


8.808 


4.775 


5.703 


-0.0777 


2.026 


5.194 


0.416 


0.318 


8.579 


4.775 


5.702 


-0.0740 * 


2.090 


5.102 


0.416 


0.317 


8.245 


4.776 


5.701 


-0.0703 


2.122 


5.100 


0.416 


0.315 


7.800 


4.777 


5.700 


-0.0666 


2.270 


5.095 


0.416 


0.313 


7.101 


4.777 


5.697 


-0.0592 


2.32 


5.094 


0.416 


0.311 


7.000 


4.778 


5.695 



Table 4 . rms estimate errors for longitudinal motion estimator 
with variation in Zw derivative. 



Zw 


u 

ft/s 


w 

ft/s 


q 

deg/ s 


0 

deg 


n 

ft 


u 

ftfs 


w 

ftfs 


-0.9612 


30.08 


6.172 


0.486 


0.440 


17.97 


4.778 


7.138 


-0.8866 


11.80 


5.810 


0.428 


0.415 


33.50 


4.785 


5.947 


-0.8463 


5/345 


5/259 


0.421 


0.400 


22.776 


4.779 


5.737 


-0.806 * 


2.090 


5.102 


0.416 


0.317 


8.245 


4.776 


5.701 


-0.7657 


2.668 


5.032 


0.412 


0.219 


16.903 


4.772 


5.710 


-0.7256 


3.188 


5.035 


0.407 


0.200 


21.026 


4.769 


5.746 


-0.665 


3.260 


5.065 


0.406 


0.185 


23.000 


4.767 


5.794 
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Table 5 . rms estimate errors for longitudinal motion estimator 
with variation in Mu derivative. 



Mu 


u 

ft/ s 


w 

ft/s 


q 

deg/s 


0 

deg 


h 

ft 


U 2 

tt/s 


"g 

ft/s 


-0.000943 


2.234 


5.061 


0.416 


0.305 


6.230 


4.775 


5.689 


-0.000865 


2.115 


5.089 


0.416 


0.310 


6.640 


4.775 


5.695 


-0.000825 


2.104 


5.094 


0.416 


0.314 


7.531 


4.776 


5.698 


-0.000786* 


2.090 


5.102 


0.416 


0.317 


8.245 


4.776 


5.701 


-0.000747 


1.993 


5.105 


0.416 


0.318 


8.595 


4.777 


5.703 


-0.000707 


1.866 


5.108 


0.416 


0.319 


8.832 


4.779 


5.705 


-0.000629 


1.566 


5.111 


0.416 


0.322 


9.178 


4.783 


5.708 



Table 6 . rms estimate errors for longitudinal motion estimator 
with variation in Mw derivative. 



M w 


u 

ft/s 


w 

ft/s 


q 

deg/s 


5 

deg 


h 

ft 


C g 
ft/ s 


w g 

ft/s 


-0.01165 


18.43 


8.350 


0.502 


0.455 


29.870 


4.778 


5.695 


-0.0113 


5.163 


5.142 


0.433 


0.373 


17.790 


4.778 


5.694 


-0.0112 


3.110 


5.113 


0.418 


0.321 


10.427 


4.777 


5.699 


-0.0111* • 


2.090 


51.102 


0.416 


0.317 


8.245 


4.776 


5.701 


-0.0109 


5.206 


5.018 


0.419 


0.325 


16.650 


4.776 


5.700 


-0.01055 


13.652 


5.342 


0.447 


0.430 


12.204 


4.776 


5.918 


-0.00999 


19.13 


18.95 


0.475 


0.704 


68.98 


4.756 


6.102 
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Table 7 . rms estimate errors for longitudinal motion estimator 
with variation in Mq derivative. • 



Mq 


u 

ft/ s 


w 

ft/ s 


q 

deg/s 


0 

deg 


h 

ft 


U g 

ft/ s 


W g 

ft/s 


-1.109 


2.091 


5.102 


0.416 


0.316 


8.230 


4.776 


5.701 


-1.016 


2.091 


5.102 


0.416 


0.316 


8.234 


4.776 


5.701 


-0.970 


2.091 


5.102 


0.416 


0.317 


8.236 


4.776 


5.701 


-0.924* 


2.090 


5.102 


0.416 


0.317 


8.245 


4.776 


5.701 


-0 . 880 


2.090 


5.102 


0.416 


0.316 


8.244 


4.776 


5.701 


-0.832 


2.089 


5.102 


0.416 


0.316 


8.236 


4.776 


5.701 


-0.7392 


2.089 


5.102 


0.416 


0.316 


8.232 


4.776 


5.701 



Table 8 . rms estimate errors for longitudinal motion estimator 
with variation in derivative. 



Mw 


u 

ft/s 


w 

ft/s 


q 

deg/ s 


0 

deg 


h 

ft 


u 

ftfs 


w 

f tf s 


-0.00061 


2.610 


5.053 


0.417 


0.273 


10.665 


4.775 


5.688 


-0.00056 


2.476 


5.089 


0.417 


0.295 


6.301 


4.776 


5.703 


-0.00053 


2.398 


5.091 


0.417 


0.304 


4.150 


4.776 


5.698 


-0.00051* 


2.090 


5.102 


0.416 


0.317 


8.245 


4.776 


5.701 


-0.00049 


2.491 


5.108 


0.416 


0.322 


9.757 


4.776 


5.702 


-0.00046 


2.976 


5.118 


0.415 


0.332 


12.067 


4.776 


5.703 


-0.00041 


3.570 


5.124 


0.415 


0.340 


13.536 


4.776 


5.703 
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Table 9 . rms estimate errors for lateral motion estimator with 
variation in Y derivate. 



Yv 


vB 

ft/s 


r 

deg/s 


P 

deg/s 


0 

deg 


deg 


Vr 


-.10416 


3.914 


0.246 


0.380 


0.236 


0.216 


5.743 


-.09548 


3.856 


0.246 


0.379 


0.231 


0.216 


5.728 


-.09110 


3.827 


0.245 


0.379 


0.228 


0.216 


5.719 


-.0868 * 


3.799 


0.245 


0.379 


0.226 


0.216 


5.719 


-.08246 


3.769 


0.245 


0.378 


0.223 


0.215 


5.702 


-.07812 


3.741 


0.245 


0.378 


0.220 


0.215 


5.694 


-.06944 


3.678 


0.245 


0.377 


0.214 


0.215 


5.677 



Table 10 . rms estimate errors for lateral motion estimator 



with variation in Ni derivative. 



N T 


V B 

f t/s 


r 

deg/ s 


P 

deg/ s 


5 

deg 


deg 


VBg 
_f t/s 


2.354 


6.889 


0.280 


0.391 


0.649 


0.233 


7.719 


2.247 


5.129 


0.266 


0.384 


0.501 


0.230 


6.220 


2.1828 


4.717 


0.253 


0.383 


0.404 


0.223 


6.052 


2.14 * 


3.799 


0.245 


0.379 


0.226 


0.216 


5.712 


2.0972 


4.125 


0.237 


0.371 


0.325 


0.217 


5.938 


2.033 


4.956 


0.224 


0.353 


0.655 


0.218 


6.543 


1.926 


8.781 


0.201 


0.299 


1.101 


0.220 


8.240 
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Table 11 . rms estimate errors for lateral motion estimator 
with variation in N’ derivative. 





v 6 

ft/s 


r 

deg/s 


P 

deg/s 


0 

deg 


deg. . 


V Bg 

ft/s 


-0.2736 


3.790 


0.248 


0.380 


0.225 


0.217 


5.704 


-0.2508 


3.791 


0.246 


0.379 


0.225 


0.216 


5.706 


-0.2394 


3.794 


0.246 


0.379 


0.225 


0 o 216 


5.710 


-0.228 * 


3.799 


0.245 


0.379 


0.226 


0.216 


5.712 


-0.2166 


3.805 


0.245 


0.379 


0.226 


0.216 


5.714 


-0.2052 


3.815 


0.246 


0.379 


0.228 


0.216 


5.719 


-0.1824 


3.834 


0.248 


0.379 


0.231 


0.216 


5.727 



Table 12 . rms estimate errors for lateral motion estimator 
with variation in Np derivative. 



N' 

P 


ft/ s 


r 

deg/s 


P 

deg/s 


0 

deg 


J 

deg 


ftjs 


-0.02448 


4.121 


0.247 


0.380 


0.318 


0.218 


5.826 


-0.02244 


3.980 


0.246 


0.380 


0.277 


0.217 


5.768 


-0.02142 


3.889 


0.246 


0.379 


0.253 


0.216 


5.742 


-0.0204 * 


3.799 


0.245 


0.379 


0.226 


0.216 


5.712 


-0.01938 


3.719 


0.245 


0.379 


0.198 


0.216 


5.745 


-0.01836 


3.708 


0.246 


0.379 


0.183 


0.216 


5.813 


-0.01632 


3.695 


0.248 


0.382 


0.173 


0.217 


6.029 
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Table 13 . rms estimate errors for lateral motion estimator 
with variation in Lg # 





v 6 

ft/s 


r 

deg/s 


P 

deg/s 


0 

deg __ 


$ 

. deg 


v 6 

fth 


-4.8510 


7.498 


0.120 


0.338 


0.885 


0.089 


2.266 


-4.6305 


5.621 


0.205 


0.360 


0.539 


0.178 


4,244 


-4.4982 


4.642 


0.240 


0.375 


0.236 


0.211 


5.529 


-4.41 * 


3.799 


0.245 


0.379 


0.226 


0.216 


5.712 


-4.3659 


4.268 


0.249 


0.382 


0.319 


0.219 


5.875 


-4.3218 


4.640 


0.250 


0.390 


0.402 


0.220 


6.295 


-4.1895 


5.246 


0.253 


0.393 


0.492 


0.224 


6.238 



Table 14. rms estimate errors for lateral motion estimator 



with variation in L^. ’ 



L r 


ft/s 


r 

deg/ s 


P 

deg/s 


9 

deg 


$ 

deg 


ft/s 


-0.3674 


12.030 


0.233 


0.352 


0.205 


0.300 


8.149 


-0.3507 


11.538 


0.236 


0.363 


0.208 


0.292 


7.910 


-0.3407 


10.712 


0.240 


0.370 


0.213 


0.276 


7.746 


-0.334 * 


3.799 


0.245 


0.379 


0.226 


0.216 


5.712 


-0.3273 


9.476 


0.209 


0.365 


1.089 


0.249 


6.613 


-0.3173 


10.712 


0.206 


0.354 


1.551 


0.252 


7.198 


-0.3006 


11.248 


0.197 


0.350 


1.986 


0.266 


7.309 
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Table 15. rms estimate errors for lateral motion estimator 



with variation in Ll. 





ye 

ft/s 


r 

deg/ s 


P 

deg/s 


0 

deg 


i> 

deg 


V 6 g 

ft/s 


-1.299 


3.846 


0.254 


0.380 


0.228 


0.219 


5.723 


-1.240 


3.812 


0.248 


0.379 


0.227 


0.217 


5.719 


-1.204 


3.799 


0.246 


0.379 


0.226 . 


0.216 


5.714 


-1.181 * 


3.799 


0.245 


0.379 


0.226 


0.216 


5.712 


-1.1573 


3.801 


0.246 


0.379 


0.226 


0.216 


5.712 


-1.122 


3.807 


0.248 


0.379 


0.226 


0.217 


5.713 


-1.063 


3.848 


0.255 


0.379 


0.227 


0.220 


5.719 



94 



oooooo oooonoooo ooooonoooooooo ooooooonooooonooo 



ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

SENSITIVITY COVARIANCE PROGRAM 

THIS PROGRAM IS USED TO SOLVE THE ERROR SENSITIVITY 
EQUATIONS WHEN THERE IS AN INCORRECT IMPLEMENTATION 
OF DYNAMICS IN THE DESIGN OF THE KALMAN FILTER. THE 
EQUATIONS BECOME 

PDOT=( F*-K*H) P+P( F*-K*H)H-OFV+VTDFT+GQGT+K*RK*T 
VDOT=FV+V (F*-K*H)T+UDFT-GQGT 
UDOT=FU+UFT +GQGT 

THE PRINCIPAL PROGRAM INPUTS ARE THE FOLLOWING CO- 
LLECTION OF SYSTEM AND FILTER MATRICES 

PO THE INITIAL COVARIANCE MATRIX! NXN ) 

F THE TRUTH MODEL DYNAMICS M ATRIX(NXN) 

F* THE FILTER MODEL DYNAMICS MATRIX(NXN) 

H THE TRUTH MODEL MEASUREMENT MATR IX ( LXN ), WHERE 
L IS THE MEASUREMENT VECTOR DIMENSION 
GQGT THE INPUT NOISE COVARIANCE MATRIX(NX) 

R THE MEASUREMENT NOISE COVARIANCE MATRIX(LXL) 

K* FILTER GAIN(NXL) 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 



THIS PROGRAM HAS BEEN DEVELOPED USING THE IMSL LIBRARY 
AVAILABLE IN THE COMPUTER CENTER OF THE NAVAL 
POSTGRADUATE SCHOOL 



IMPLICIT REAL *8 (A-H,0-Z) 

COMMON F< 7,7) ,FS(7,7) , GQGT! 7) ,AK(7,2),R(2,2), 
*AKRKT(7,7), DF(7,7) ? FT! 7,7) ,FSMKHT(7,7> ,DFT!7,7) , 
*FSMKH(7,7 ) ,H( 2,7) 

COMMON/KTR/N » NS,NPD 
DIMENSION PFULL ( 7 , 7 ) ,P SQR ( 7 ) 

DIMENSION DDT (7) 

DIMENSION U(28),V(7,7) ,P(28),UD!28) ,VD(7,7> , 

♦V AR ( 105 ) , DRV! 105 ) , C I 24 ) , WK( 10 5 , 9 ) , PD ( 28 ) 

DIMENSION TMP 1 { 7 , 7 ) ,TMP2!7,7) ,TMP3(7,7) 
EQUIVALENCE ( U( 1 ) , VAR! 1 ) ) , ! V t 1 , 1 ) , VAR! 29 ) ) . ( P ( 1 ) , 
*VAR(78) ), (UD( 1) ,DRV ! 1 ) > , ( VD < 1 , 1 ) , DRV ( 29 ) > , ( PD ( 1 ) , 
♦DRV (78) ) 

N=ORDER OF THE SYSTEM MODEL 
NP=NUMBER OF POINTS 

NPD=CONTROL OF INITIAL DIAGNOSTIC OUTPUT 

D T= T I ME INTERVAL 

EXTERNAL FUN 
CALL UGET 10(3,5,6) 



THE FOLLOWING SECTION READS THE SPECIFIED INPUT 
MATRICES, F,F*, GQGT, K*H AND R 



READ(5,98 )N,NP,NPD, DT 
98 FORMAT(3I5»F10.5) 

WRITE(6,97)N,NP,NPD,DT 
97 FORMAT! *1 • ,315, IX, G18. 10) 
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ooon 



NS=N*< N+l )/2 
N1=NS+N**2+1 
NV=2*NS+N**2 
99 FORMAT ( 8 F10 . 5 ) 

00 1 1 = 1, N 

1 RE AD ( 5 , 99 ) (F(I,J),J=1»N) 

CALL USWFMC *F« ,i,F »7,N,N,i) 

DO 2 1 = 1, N 

2 RE AD( 5 , 99 ) ( F SC I , J > , J= 1 ,N I 
CALL USWFMC »FS*, 2, FS,7,N,N, 1) 

READC 5,99) ( GQGT ( I ) , 1=1 ,N J 
CALL USWFVC •GQGT* ,4, GQGT, N, 1,1) 

DO 3 1=1, N 

3 READC 5, 99 ) C AK( I , J I , J=1 ,2 ) 

CALL USWFMC 'K' ,1,AK,7,N,2,1 I 

DO 4 1=1,2 

4 RE ADC 5 , 99 ) ( H ( I , J I , J = 1 , N ) 

CALL USWFMC'H' , 1 , H , 2 ,2 , N, 1 1 

DO 5 1=1,2 

5 READC 5 ,99 ) C R ( I , J ) , J=1 , 2 I 
CALL USWFMC »R», 1,R,2,2,2,1) 

DO 6 1=1,105 

6 VAR C I ) = 0. 

DO 7 1 = 1, N 
DO 7 J=1 » N 

7 DFC I,JI=FS( I, J)-F( I ,J) 

CALL USWF MC ' DEL F* , 5, OF »7,N»N ,1 1 
CALL VMUL FF C AK» R , N * 2, 2 » 7, 2, TMPi » 7 » I ER) 

CALL VMULFPC TMP1 . AK , N, 2 ,N , 7 , 7 , AKRKT, 7 , IER ) 

CALL USWFMC 1 KRKT* , 4, AKRKT ,7 ,M ,N , 1 ) 

CALCULATE THE DIFFERENCE BETWEEN THE DYNAMICS 
IMPLEMENTED IN THE FILTER AND THE PLANT, DF=F*-F 

DO 20 1=1 ,N 
DO 20 J=1,N 
DFT ( I » J ) = DF ( I , J ) 

20 FTCI,J)=F(I,J) 

CALL VTRANX(DFT,N,N,7) 

CALL USWFMC • DEL FT • ,6, DFT , 7 , N ,N , 1 ) 

CALL VTRANXCFT,N,N,7) 

CALL USWFMC * FT', 2, FT, 7 ,N,N, 1) 

CALL VMULFFCAK,H,N,2,N,7,2, TMP1,7,IER) 

DO 21 1=1,7 
DO 21 J=1 ,7 

FSMKHCI ,J)=FS(I, J)-TMP1(I,JI 

21 FSMKHTCI, J)=FSMKH( I ,J) 

CALL USWFMC 'FS-KH' , 5,FSMKH,7,N,N, 1) 

CALL VTRANX ( F SMKHT , N,N , 7 ) 

CALL USWFMC ' C FS-KH )T* , 8 , FSMKHT , 7 , N , N , 1 ) 

T=0 . 

T0L=l.D-5 

IND=1 

L=0 

IF CN.EQ.7 ) GO TO 11 

DO 31 1=1, NS 
L=L + 1 

31 VAR(U=UC II 
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c 



c 



C 

C 



DO 32 1 = 1 * N 
DO 32 J=1 »N 
L=L + 1 

32 VAR ( L) =V ( I v J) 

DO 33 1=1, N 
1=1 + 1 

33 VAR(L)=P< I) 

11 DO 10 K= 1 , NP 

TEND=FINAL TIME 

TENC=K*DT 

DVERK SUBROUTINE FINDS THE SOLUTION OF THE SYSTEM OF 
DIFFERENTIAL EQUATIONS 

CALL DVERK! NV ,FJN» T,VAR »TEND, TOL * IND»C» 105 ,WK, I ER ) 

IF ( IND.LE.O.OR. IER . NE . 0 ) STOP 
CALL VCVTSF(VAR!N1) ,N,PFULL,7 ) 

CALCULATE AND PRINT THE RMS ESTIMATE ERRORS 

DO 30 1=1, N 

30 PSQR(I)=OSQRT!PFJLL(I,I)l 
WRITE! 6,90) T, (PSQR ( I) ,1=1 ,N) 

90 FORMAT! «0T=» ,F10. 5, * P SR= • , 70 15. 7 ) 

IF DESIRED PRINT THE COVARIANCE MATRICES, P,U AND V 
CALL USWSM! • U • , 1 , 'J , N, 2 ) 

CALL USWFM! »V* ,1,VAR(NS+1) ,7,N,N,2) 

CALL USWSM! *? *, 1, VAR(Nl) ,N,2) 

10 CONTINUE 
STOP 
END 
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SUBROUTINE VTRA'J X ( A , N, NC, IA ) 
IMPLICIT REAL *8 (A-H,0-Z> 
DIMENSION A ( I At I A ) » B( 7 » 7 ) 

C 

DO l 1=1, N 
DO 1 J = 1 » N 
I B( I » J)=A( J, I ) 

C 

DO 2 1=1, N 
DO 2 J=1,N 
2 A( I , J) = B( I, J) 

RETURN 

END 
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SUBROUTINE FUNH V, T .VAR.ORV) 

FCN SUBROUTINE IS USED FOR EVALUATING FUNCT IONS ( INPUT) 
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5 



IMPLICIT REAL*8 (A-H,0-Z) 

COMMON F{ 7,7 ) ,FS( 7, 7) ,GQGTt 7) ,AK(7,2),Rt2,2), 

#AKRKT ( 7 » 7 ) ,DF(7,7) , FT( 7 , 7 ) , FSMKHT ( 7 , 7 ) ,DFTt7,7), 
*FSMKH ( 7 » 7 ) , H( 2,7) 

COMMON/KTR/N,NS,NPD 

DIMENSION U(28> ,V(7,7) ,P<28> , UD(28) ,VD(7,7) , 

*VAR ( 105 ) » DRVt 105) * C ( 24) ,WK( 105, 9) , PD < 28) 

DIMENSION TMP It 7, 7) ,TMP2(7,7) ,TMP3t7,7) 

L=0 

DO 1 1=1, NS 
L = L + i 

. U( I ) =VAR( L ) 

DO 2 1=1, N 
DO 2 J=l, N 
L=L + 1 

! Vt I , J)=VAR( L) 

DO 3 1=1, NS 
L=L + 1 

. P( I )=VAR( L ) 

IF(T.EQ.O) KT=NPD 
KT=KT+1 

IF (KT.GE.5 ) GO TO 15 
WR I TE ( 6 , 99) T 
FORMAT ( '0T= ' ,025.15) 

CALL USWSMt «U« ,1,U, N,2) 

CALL USWFMI *V*,1,V,7,N»N,2) 

CALL USWSMt • P ' , 1, V , N, 2 ) 

CALL VMULFS ( F,U,N,N,7,TMPL,7) 

IF ( KT.LT. 5) CALL U S WFM { • FU» , 2 , TMP 1 , 7 ,N , N , 2 ) 

CALL VMULSF(U,N,FT,N,7,TMP2,7) 

IF ( KT.LT. 5) CALL USWFMt *UFT • , 3,TMP2 ,7,N, N, 2 ) 

DO 5 1=1, N 
DO 4 J=1,N 

TMP It I, J)=TMP1( I, J)+TMP2(I,J) 

TMP1 { I , I ) =TMP1 { I , I ) +GQGT ( I ) 

CALL VCVTFStTMPl ,N,7,UD) 

IFIKT.LT.5) CALL US WSM ( • UDOT* , 4, UD , N ,2 ) 

CALL VMULFF (F,V»N»N,N,7.7, TMP 1,7, IER ) 

IF ( KT.LT. 5) CALL USWFMt'FV' ,2,TMPi,7,N,N,2) 

CALL VMULFF(V,FSMKHT,N,N,N,7,7,TMP2,7, IER) 

IF ( KT.LT. 5 ) CALL USWFMt *V*tFS-KH)T' , 10,TMP2,7 ,N,N,2 ) 
CALL VMULSF(U,N,DFT ,N,7 ,TMP3, 7) 

IFtKT.LT.5) CALL USWFMt , U*DFT',5,TMP3,7,N,N,2) 



1 = 1, N 

J=1 , N 

i J)=TMPlt I,J)+TMP2tI,J)+TMP3t I, J) 

1 1 ) =V 0 ( I , D-GQGT 1 1 ) 

’. LT. 5) CALL USWFMt • VDOT' ,4,VD,7,N,N, 2 ) 

. VMJLFSt FSMKH,P,N,N,7, TMPi ,7 ) 

’ .LT. 5 ) CALL ,USWFM( ' i FS-KH) P ' , 8 , TMP 1 , 7 , N , N, 2 ) 
VMULSF t P »N,F SMKHT , N, 7, TMP2 , 7 ) 

'.LT. 5 ) CALL USWFM('P(FS-KH)T , ,9,TMP2,7,N,N»2) 
VMULFF tDF,V,N, N,N,7,7, TMP3,7 , IER) 
f.LT.5) CALL USWFMt »DF#V' ,4 , TMP3 ,7 ,N , N , 2 ) 



DO 8 I =1 , N 
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c 



c 

c 

c 



DO 8 J = 1 , N 

8 TMPICIf J)=TMPl(I, Ji+TMP2 ( It J)+TMP3< I,J) 

CALL VMULFM(VtDF,N,N,N,7t7,TMP3t7tI6R) 
IF(KT.LT.5J CALL US WFM < • VT*DF • , 5 , TMP3 » 7 , N , N , 2 ) 

DO 10 I=i,N 
DO 9 J = l, N 

9 TMP3I I , J) =TMP I ( I, J) +TMP3I I • J ) +AKRKT ( I » J ) 

10 TMP3( 1 1 1 ) =TMP3( I t I ) +GQGT( I ) 

IFIKT.LT. 5) CALL US WFM ( 'PDOT' »4tTMP3t7»N»N»2) 
CALL VCVTFSITMP3 ,N»7tPD) 

IF ( KT .LT • 5) CALL USWFV ( • PDOT ( SYM) • t 9 t PD»N » I t 2 ) 
L=0 



DO II 1 = 1, NS 
L=L + 1 

11 DRV ( L) =UD ( I ) 

DO 12 1=1 ,N 
DO 12 J = 1 , N 
L=L + 1 

12 DRV (L)=VD (I,J) 

DO 13 1=1, NS 
L=L + 1 

13 DRV (L) =PD ( I ) 

IFIKT.LT. 5) CALL U SWFV ( 'DRV • , 3 , DRV , NV , 1 , 2 ) 

RETURN 

END 
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